#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <image_transport/subscriber.h>
#include <image_transport/image_transport.h>

#include <cv_bridge/CvBridge.h>
#include <opencv2/highgui/highgui.hpp>
//#include "window_thread.h"

#include <boost/thread.hpp>
#include <boost/format.hpp>

#ifdef HAVE_GTK
#include <gtk/gtk.h>
#endif



class Viewer {
public:
	Viewer(std::string );
	virtual ~Viewer();
	void callBack(const sensor_msgs::ImageConstPtr& msg);

	image_transport::Subscriber sub_;
	sensor_msgs::CvBridge img_bridge_;

	boost::mutex image_mutex_;
	sensor_msgs::ImageConstPtr last_msg_;
	cv::Mat last_image_;


	std::string window_name;
	boost::format filename_format_;
	int count_;


};

Viewer::Viewer(std::string windowName) {
	window_name = windowName;

}

Viewer::~Viewer() {
	// TODO Auto-generated destructor stub
}

void Viewer::callBack(const sensor_msgs::ImageConstPtr& msg)
{
	ROS_WARN("####Encoding:");
	ROS_WARN(msg->encoding.c_str());
	image_mutex_.lock();

	// May want to view raw bayer data, which CvBridge doesn't know about
	if (msg->encoding.find("bayer") != std::string::npos)
	{
		last_image_ = cv::Mat(msg->height, msg->width, CV_8UC1,
				const_cast<uint8_t*>(&msg->data[0]), msg->step);
	}
	// We want to scale floating point images so that they display nicely
	else if(msg->encoding.find("F") != std::string::npos)
	{

		ROS_WARN("####Floating:");
		/*
		 cv::Mat  bridge;
		 try {
		 bridge = img_bridge_.imgMsgToCv(msg, "passthrough");
		 }
		 catch (sensor_msgs::CvBridgeException& e)
		 {
		 ROS_ERROR("Failed to transform depth image.");
		 return;
		 }

		 double min_range_=0.5;
		 double max_range_=5.5;

		 // convert to something visible
		 cv::Mat img(bridge.rows, bridge.cols, CV_8UC1);
		 for(int i = 0; i < bridge.rows; i++)
		 {
		 float* Di = bridge.ptr<float>(i);
		 char* Ii = img.ptr<char>(i);
		 for(int j = 0; j < bridge.cols; j++)
		 {
		 Ii[j] = (char) (255*((Di[j]-min_range_)/(max_range_-min_range_)));
		 }
		 }
		 last_image_ = img;
		 //*/

		cv::Mat float_image_bridge = img_bridge_.imgMsgToCv(msg, "32FC1");
		cv::Mat_<float> float_image = float_image_bridge;
		float max_val = 0;
		for(int i = 0; i < float_image.rows; ++i)
		{
			for(int j = 0; j < float_image.cols; ++j)
			{
				max_val = std::max(max_val, float_image(i, j));
			}
		}

		if(max_val > 0)
		{
			float_image /= max_val;
		}
		last_image_ = float_image;
	}
	else
	{
		// Convert to OpenCV native BGR color
		try {
			last_image_ = img_bridge_.imgMsgToCv(msg, "bgr8");
		}
		catch (sensor_msgs::CvBridgeException& e) {
			ROS_ERROR("Unable to convert '%s' image to bgr8",
					msg->encoding.c_str());

		}
	}

	// last_image_ may point to data owned by last_msg_, so we hang onto it for
	// the sake of mouseCb.
	last_msg_ = msg;

	// Must release the mutex before calling cv::imshow, or can deadlock against
	// OpenCV's window mutex.
	image_mutex_.unlock();
	if (!last_image_.empty()) {
		cv::imshow(window_name, last_image_);
		cv::waitKey(1);
	}
}

